{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Box measurements demo\n",
    "\n",
    "## Idea\n",
    "  * Find a ground plane, find a plane for the top of the box\n",
    "  * Find a minimum rectangle for the top side (and four points)\n",
    "  * Find the bottom four corners of the box (located on the ground plane).\n",
    "\n",
    "## What is needed for a bare minimum working demo:\n",
    " * Crop only for the ROI *DONE*\n",
    " * Detect the ground plane, either using calibration (detect the background without the box) or if\n",
    "the box is small enough, do the fit using the box. *DONE*\n",
    "\n",
    " * Find a rotation matrix to align the ground plane with the coordinate system. *DONE*\n",
    "\n",
    " * Find points not on the plane and perform clustering, in good conditions, the biggest cluster should be the box. *DONE*\n",
    "\n",
    " * Order the points by the distance to the ground plane *DONE*\n",
    "\n",
    " * Take the 0.90% of the point furthest away from the plane *DONE*\n",
    "\n",
    " * Make a new plane parallel to the ground plane *NOT NEEDED*\n",
    "\n",
    " * Do a projection of the points that are in the +- x% of the new plane on the new plane\n",
    "\n",
    " * Find a bounding box around the projected points**\n",
    "\n",
    " * Get the 4 coordinates of the bounding box\n",
    "\n",
    " * Get the 4 coordinates on the ground plane by projection\n",
    "\n",
    " * By here the demo should be able to measure the box dimensions\n",
    "\n",
    "\n",
    "## What else is needed for the full demo:\n",
    " * Figure out an reverse transformation to the RGB image to draw the box on the image\n",
    " * Gather more than one point cloud and (average/combine) them.\n",
    " * Create a calibration setup\n",
    "\n",
    "## Helper funtions:\n",
    " * A function that creates a point cloud for a plane in a given volume would be handy for visualisation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Imports and constants\n",
    "\n",
    "import open3d as o3d\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import copy\n",
    "import cv2\n",
    "import random\n",
    "\n",
    "id = 1\n",
    "PATH_DIR = './example_pcls'\n",
    "PATH = f\"{PATH_DIR}/example_{id}.ply\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Format = auto\n",
      "Extension = ply\n"
     ]
    }
   ],
   "source": [
    "# Read the pointcloud\n",
    "raw_pcl = o3d.io.read_point_cloud(PATH)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Crop the ROI\n",
    "----"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Show the pointcloud\n",
    "o3d.visualization.draw_geometries([raw_pcl])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Crop only for the ROI\n",
    "\n",
    "raw_pcl_np = np.asarray(raw_pcl.points)\n",
    "\n",
    "# Calculate point distances\n",
    "pcl_dist = np.sqrt(np.sum(np.square(raw_pcl_np), axis=1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Plot a histogram of distances\n",
    "plt.hist(pcl_dist)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Only take points less than 1 meter away\n",
    "indices = np.nonzero(pcl_dist < 1.5)[0]\n",
    "roi_pcl = raw_pcl.select_by_index(indices)\n",
    "\n",
    "o3d.visualization.draw_geometries([roi_pcl])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Detect the ground plane\n",
    "------"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get the ground plane\n",
    "plane_eq, plane_inliers = roi_pcl.segment_plane(0.02, 3, 3000)\n",
    "\n",
    "\n",
    "# Draw the datapoints in the plane\n",
    "plane_raw = roi_pcl.select_by_index(plane_inliers)\n",
    "o3d.visualization.draw_geometries([plane_raw])\n",
    "\n",
    "# Draw the outliers\n",
    "plane_outliers = roi_pcl.select_by_index(plane_inliers, invert=True)\n",
    "o3d.visualization.draw_geometries([plane_outliers])\n",
    "# TODO, draw a plane, to compare visually how this works\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Find the box"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[Open3D DEBUG] Precompute Neighbours\n",
      "[Open3D DEBUG] Done Precompute Neighbours\n",
      "[Open3D DEBUG] Compute Clusters\n",
      "[Open3D DEBUG] Done Compute Clusters: 11\n",
      "point cloud has 11 clusters\n"
     ]
    }
   ],
   "source": [
    "# Cluster the outliers, the biggest in good conditions should be the box\n",
    "with o3d.utility.VerbosityContextManager(\n",
    "    o3d.utility.VerbosityLevel.Debug) as cm:\n",
    "    labels = np.array(plane_outliers.cluster_dbscan(eps=0.02, min_points=10))\n",
    "    \n",
    "\n",
    "max_label = labels.max()\n",
    "print(f\"point cloud has {max_label + 1} clusters\")\n",
    "colors = plt.get_cmap(\"tab20\")(labels / (max_label if max_label > 0 else 1))\n",
    "colors[labels < 0] = 0\n",
    "plane_outliers.colors = o3d.utility.Vector3dVector(colors[:, :3])\n",
    "o3d.visualization.draw_geometries([plane_outliers])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Visualize the biggest cluster (hopefully the box)\n",
    "labels_short = labels[labels != -1]\n",
    "y = np.bincount(labels_short).argmax()\n",
    "box_indices = np.where(labels == y)[0]\n",
    "box = plane_outliers.select_by_index(box_indices)\n",
    "o3d.visualization.draw_geometries([box])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Helper functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Useful for debugging (see what RANSAC is finding visually)\n",
    "\n",
    "def create_pointcloud_plane(plane_params, x_range, y_range, step):\n",
    "    \"\"\"\n",
    "    Create a pointcloud from the plane parameters and ranges\n",
    "         Parameters:\n",
    "         --------------\n",
    "         plane_params -> list() 4x1\n",
    "         x, y ,z range -> list 2x1 ([min, max])\n",
    "\n",
    "         Returns:\n",
    "         point_cloud\n",
    "    \n",
    "    \"\"\"\n",
    "    pcl = o3d.geometry.PointCloud()\n",
    "    a, b, c, d = plane_params\n",
    "    x_start, x_stop = x_range\n",
    "    y_start, y_stop = y_range\n",
    "    num_points_x = int((x_stop - x_start) / step) + 1\n",
    "    num_points_y = int((y_stop - y_start) / step) + 1\n",
    "    x_points = np.linspace(x_start, x_stop, num_points_x)\n",
    "    y_points = np.linspace(y_start, y_stop, num_points_y)\n",
    "    num_points = len(x_points) * len(y_points)\n",
    "    pcl_np = np.ones((num_points, 3))\n",
    "    i = 0\n",
    "    for x in x_points:\n",
    "        for y in y_points:\n",
    "            z = (-d - (a * x) - (b * y)) / c\n",
    "            pcl_np[i] = np.array([x, y, z])\n",
    "            i += 1\n",
    "\n",
    "\n",
    "\n",
    "    pcl.points = o3d.utility.Vector3dVector(pcl_np)\n",
    "\n",
    "    return pcl\n",
    "\n",
    "def fit_plane_vec_constraint(norm_vec, pts, thresh=0.05, n_iterations=1000):\n",
    "    best_eq = []\n",
    "    best_inliers = []\n",
    "\n",
    "    n_points = pts.shape[0]\n",
    "    for iter in range(n_iterations):\n",
    "        id_sample = random.sample(range(0, n_points), 1)\n",
    "        point = pts[id_sample]\n",
    "        d = -np.sum(np.multiply(norm_vec, point))\n",
    "        plane_eq = [*norm_vec, d]\n",
    "        pt_id_inliers = get_plane_inliers(plane_eq, pts, thresh)\n",
    "        if len(pt_id_inliers) > len(best_inliers):\n",
    "            best_eq = plane_eq\n",
    "            best_inliers = pt_id_inliers\n",
    "\n",
    "    return best_eq, best_inliers\n",
    "\n",
    "def fit_plane_z0(pts, thresh=0.05, n_iterations=1000):\n",
    "    best_eq = []\n",
    "    best_inliers = []\n",
    "    # Fit the plane where a constraint is for the z component of\n",
    "    # the normal vector is to be 0\n",
    "\n",
    "    n_points = pts.shape[0]\n",
    "    for iter in range(n_iterations):\n",
    "        id_samples = random.sample(range(0, n_points), 2)\n",
    "        points = pts[id_samples]\n",
    "\n",
    "        # Get the line between two points\n",
    "        vec_line = points[1, :2] - points[0, :2]\n",
    "\n",
    "        # Normal vector is perpendicular\n",
    "        vecC = [vec_line[1], -vec_line[0], 0]\n",
    "\n",
    "        # Normalize\n",
    "        norm_vec = vecC / np.linalg.norm(vecC)\n",
    "\n",
    "        point = points[0, :]\n",
    "        d = -np.sum(np.multiply(norm_vec, point))\n",
    "        plane_eq = [*norm_vec, d]\n",
    "        pt_id_inliers = get_plane_inliers(plane_eq, pts, thresh)\n",
    "        if len(pt_id_inliers) > len(best_inliers):\n",
    "            best_eq = plane_eq\n",
    "            best_inliers = pt_id_inliers\n",
    "\n",
    "    return best_eq, best_inliers\n",
    "\n",
    "def get_plane_inliers(plane_eq, pts, thresh=0.05):\n",
    "    pt_id_inliers = []\n",
    "    dist_pt = get_pts_distances_plane(plane_eq, pts)\n",
    "\n",
    "    # Select indexes where distance is bigger than the threshold\n",
    "    pt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0]\n",
    "    return pt_id_inliers\n",
    "\n",
    "def get_intersection_three_planes(planes_eq):\n",
    "    A, B, C = planes_eq\n",
    "    line_dir, line_point = get_intersection_two_planes([A,B])\n",
    "\n",
    "    plane_normal = np.array(C[0:2])\n",
    "\n",
    "    ndotu = plane_normal.dot(line_dir)\n",
    "    plane_point = [0,0,0] #TODO\n",
    "    w = line_point - plane_point\n",
    "    si = - np.array(C[0:2]).dot(w) / ndotu\n",
    "\n",
    "    point = w + si * line_dir + plane_point\n",
    "    return point\n",
    "\n",
    "\n",
    "def norm2(X):\n",
    "    return np.sqrt(np.sum(X ** 2))\n",
    "\n",
    "def normalized(X):\n",
    "    return X / norm2(X)\n",
    "\n",
    "def get_intersection_two_planes(planes_eq):\n",
    "    A, B = planes_eq\n",
    "    U = normalized(np.cross(A[:-1], B[:-1]))\n",
    "    M = np.array((A[:-1], B[:-1], U))\n",
    "    X = np.array((-A[-1], -B[-1], 0.))\n",
    "    return U, np.linalg.solve(M, X) # [a,b,c], [x0, y0, z0]\n",
    "\n",
    "def get_pts_distances_plane(plane_eq, pts):\n",
    "    dist_pt = (plane_eq[0] * pts[:, 0] + plane_eq[1] * pts[:, 1] \n",
    "               + plane_eq[2] * pts[:, 2] + plane_eq[3])\\\n",
    "               / np.sqrt(plane_eq[0] ** 2 + plane_eq[1] ** 2 + plane_eq[2] ** 2)\n",
    "    return dist_pt\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "# pointcloud = create_pointcloud_plane([0, 0, 1, -1], [0,2], [-2,2], 0.02)\n",
    "\n",
    "# coord = o3d.geometry.TriangleMesh.create_coordinate_frame()\n",
    "# pointcloud.paint_uniform_color([0,255,0])\n",
    "# o3d.visualization.draw_geometries([pointcloud, coord])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Fit the box with pyRansac cube fit and visualise the result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pyransac3d as pyrsc\n",
    "\n",
    "box_points = np.asarray(box.points)\n",
    "cube = pyrsc.Cuboid()\n",
    "best_eq, best_inliers = cube.fit(box_points, thresh=0.015, maxIteration=3000)\n",
    "\n",
    "\n",
    "box_insiders = box.select_by_index(best_inliers)\n",
    "o3d.visualization.draw_geometries([box_insiders])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "How many percent inliers: 94.94163424124513%\n"
     ]
    }
   ],
   "source": [
    "percent_inliers = 100 * (len(best_inliers) / len(box.points))\n",
    "print(f\"How many percent inliers: {percent_inliers}%\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.0391963 , -0.11599102, -0.94971429],\n",
       "       [ 0.04938117, -0.11298155, -0.95066669],\n",
       "       [ 0.05686342, -0.10971594, -0.954     ],\n",
       "       ...,\n",
       "       [ 0.01650611, -0.05068626, -0.94499999],\n",
       "       [ 0.07086273, -0.05569545, -0.9409    ],\n",
       "       [ 0.07983237, -0.05480072, -0.94825   ]])"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "box_points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "per_inliers_per_plane = []\n",
    "for eq in best_eq:\n",
    "    box_inliers = get_plane_inliers(eq, box_points, 0.015)\n",
    "    per_inliers_per_plane.append(len(box_inliers) / len(box_points) * 100)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[81.90661478599222, 17.898832684824903, 13.715953307392997]"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "per_inliers_per_plane"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Visualize the box planes\n",
    "\n",
    "planes = []\n",
    "for i, eq in enumerate(best_eq):\n",
    "    if per_inliers_per_plane[i] > 5:\n",
    "        planes.append(create_pointcloud_plane(eq, [-1, 1], [-1, 1], 0.01))\n",
    "\n",
    "o3d.visualization.draw_geometries(planes + [box])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Rotate and translate the ground plane\n",
    "Rotate the ground plane along the x or y axis so the normal vector\n",
    "of the ground plane is only positive in the z direction.\n",
    "The translate the ground plane to have zero distance with the origin."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Calculate the rotation matrix\n",
    "\n",
    "def rotate_vector(vec_in, vec_target):\n",
    "    # Create a rotation matrix that rotates vec_in to vec_target\n",
    "    # https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d\n",
    "    v = np.cross(vec_in, vec_target)\n",
    "    s = np.linalg.norm(v)\n",
    "    c = np.matmul(vec_in, vec_target)\n",
    "    v_mat = np.array([[0    ,-v[2], v[1] ],\n",
    "                      [v[2] , 0   , -v[0]],\n",
    "                      [-v[1], v[0], 0    ]])\n",
    "    R = np.identity(3) + v_mat + (np.matmul(v_mat, v_mat) * (1 / (1+c)))\n",
    "    return R\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "norm_vector = plane_eq[0:3]\n",
    "wanted_norm_vector = np.array([0, 0, 1])\n",
    "rot_matrix = rotate_vector(norm_vector, wanted_norm_vector)\n",
    "\n",
    "# Create visible coordinates\n",
    "coord = o3d.geometry.TriangleMesh.create_coordinate_frame(size=.1)\n",
    "rotated_plane = copy.deepcopy(plane_raw)\n",
    "rotated_plane = rotated_plane.rotate(rot_matrix, center=(0,0,0))\n",
    "o3d.visualization.draw_geometries([coord, rotated_plane])\n",
    "\n",
    "\n",
    "avg_z = np.average(np.asarray(rotated_plane.points)[:, 2])\n",
    "translated_plane = rotated_plane.translate([0, 0, - avg_z])\n",
    "o3d.visualization.draw_geometries([coord, rotated_plane])\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-5.02315444e-18, -1.78362156e-17,  1.00000000e+00])"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Check that the normal vector really is pointing up\n",
    "np.matmul(rot_matrix , norm_vector)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Draw all z values\n",
    "points = np.asarray(rotated_plane.points)\n",
    "plt.plot(points[:,2])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7fd654440f10>]"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.plot(points[:,0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Order the points and take the ones furthest away from the plane"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Rotate and translate also the box\n",
    "\n",
    "rotated_box = copy.deepcopy(box)\n",
    "rotated_box = rotated_box.rotate(rot_matrix, center=(0,0,0))\n",
    "translated_box = rotated_box.translate([0, 0, - avg_z])\n",
    "o3d.visualization.draw_geometries([coord, translated_box, translated_plane])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Fit the z plane\n",
    "translated_box_points = np.asarray(translated_box.points)\n",
    "top_plane_eq, top_plane_inliers = fit_plane_vec_constraint([0, 0, 1], translated_box_points, 0.03, 30)\n",
    "top_plane = translated_box.select_by_index(top_plane_inliers)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Fit another plane\n",
    "side_plane_eq, side_plane_inliers = fit_plane_z0(translated_box_points, 0.015, 100)\n",
    "side_plane = translated_box.select_by_index(side_plane_inliers)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "# points_np = np.asarray(translated_box.points)\n",
    "# zs = points_np[:, 2]\n",
    "# sorted_zs = np.sort(zs)\n",
    "# plt.plot(sorted_zs)\n",
    "# percentile90 = np.percentile(sorted_zs, 90)\n",
    "height = - top_plane_eq[3]\n",
    "\n",
    "o3d.visualization.draw_geometries([translated_plane, top_plane, side_plane]) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Rotate to get the box aligned to coordinate system\n",
    "R = rotate_vector(side_plane_eq[0:3], [0,1,0])\n",
    "aligned_box = copy.deepcopy(rotated_box)\n",
    "aligned_box = aligned_box.rotate(R, center=(0,0,0))\n",
    "aligned_box = aligned_box.translate([0, side_plane_eq[3], 0])\n",
    "coord = o3d.geometry.TriangleMesh.create_coordinate_frame(size=.1)\n",
    "\n",
    "o3d.visualization.draw_geometries([aligned_box, coord])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7fd6543c3160>]"
      ]
     },
     "execution_count": 28,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "aligned_box_points = np.asarray(aligned_box.points)\n",
    "ys = abs(aligned_box_points[:, 1])\n",
    "ys.sort()\n",
    "width = np.percentile(ys, 98)\n",
    "plt.plot(ys)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7fd654322f70>]"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "xs = abs(aligned_box_points[:, 0])\n",
    "xs.sort()\n",
    "xs = xs - min(xs)\n",
    "length = np.percentile(xs, 98)\n",
    "plt.plot(xs)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.17065599192754308 0.285807461148093\n"
     ]
    }
   ],
   "source": [
    "print(width, length)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Bounding box on the 2D points\n",
    "rect = cv2.minAreaRect(points)\n",
    "points = cv2.boxPoints(rect)\n",
    "bottom_points = points, just reducted z by the distance between the planes\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [],
   "source": [
    "upper_plane_points = np.asarray(top_plane.points)\n",
    "coordinates = np.c_[upper_plane_points[:, 0], upper_plane_points[:, 1]].astype('float32')\n",
    "rect = cv2.minAreaRect(coordinates)\n",
    "bounding_box = cv2.boxPoints(rect)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Visualise the box borders"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [],
   "source": [
    "points_floor = np.c_[bounding_box, np.zeros(4)]\n",
    "points_top = np.c_[bounding_box, height * np.ones(4)]\n",
    "box_points = np.concatenate((points_top, points_floor))\n",
    "\n",
    "\n",
    "rotated_whole_pcl = copy.deepcopy(raw_pcl)\n",
    "rotated_whole_pcl = rotated_whole_pcl.rotate(rot_matrix, center=(0,0,0))\n",
    "translated_whole_pcl = rotated_whole_pcl.translate([0, 0, -avg_z])\n",
    "\n",
    "\n",
    "lines = [\n",
    "    [0, 4],\n",
    "    [1, 5],\n",
    "    [2, 6],\n",
    "    [3, 7],\n",
    "    [0,1],\n",
    "    [1,2],\n",
    "    [2,3],\n",
    "    [3,0],\n",
    "    [4,5],\n",
    "    [5,6],\n",
    "    [6,7],\n",
    "    [7,4],\n",
    "]\n",
    "\n",
    "line_set = o3d.geometry.LineSet(\n",
    "    points = o3d.utility.Vector3dVector(box_points),\n",
    "    lines = o3d.utility.Vector2iVector(lines),\n",
    ")\n",
    "\n",
    "colors = [[1,0,0] for i in range(len(lines))]\n",
    "line_set.colors = o3d.utility.Vector3dVector(colors)\n",
    "\n",
    "bdbox = line_set.get_oriented_bounding_box()\n",
    "o3d.visualization.draw_geometries([bdbox, translated_whole_pcl])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [],
   "source": [
    "width, length, height = rect[1][0], rect[1][1], height"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.2622608542442322 0.2787065804004669 0.09591756493561943\n"
     ]
    }
   ],
   "source": [
    "print(width, length, height)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.12 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.4"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
